#include <cmath>
#include <ctime>
#include <iostream>
#include "RBF.h"
#include "PBCG.h"
#include "SVD.h"

RBF::RBF()
{
  ps = NULL;
  ownPointSet = false;
	tree = NULL;
	sol = NULL;
	sub_rbf = NULL;
	isGlobal = false;
	c = 0.0;
  minSupport = 0;
}

RBF::~RBF()
{
  if(ownPointSet && ps != NULL)
    delete ps;
	if(tree != NULL)
		delete tree;
	if(sol != NULL)
		delete sol;
	if(sub_rbf != NULL)
		delete sub_rbf;
}


int RBF::numSupportPoints(float x, float y, float z)
{
  int count = 0, size;

	if(sub_rbf != NULL)
		count += sub_rbf->numSupportPoints(x, y, z);

  tree->getIndexTable(size, x, y, z, support);
  count += size;

  return count;
}


float RBF::value(float x, float y, float z)
{
	double f = 0;

	if(isGlobal){
		return -1; 
	}
	if(sub_rbf != NULL)
		f += sub_rbf->value(x, y, z);
	
	int size, j;
	const int* table = tree->getIndexTable(size, x, y, z, support);
	float vx, vy, vz;
	double d;
	if(!ps->isQuad()){
		for(int i=0; i<size; i++){
			j = table[i];
			vx = x - ps->getPointX(j);
			vy = y - ps->getPointY(j);
			vz = z - ps->getPointZ(j);
			d = ps->getNormalX(j)*vx + ps->getNormalY(j)*vy + ps->getNormalZ(j)*vz;

			f += (d/sqrt(K2*d*d+1.0) + sol[j+1])*weight(vx*vx + vy*vy + vz*vz);
		}
	}
	else{
		float lx, ly, lz;
		for(int i=0; i<size; i++){
			j = table[i];
			vx = x - ps->getPointX(j);
			vy = y - ps->getPointY(j);
			vz = z - ps->getPointZ(j);
			lx = ps->getTangent1X(j)*vx + ps->getTangent1Y(j)*vy + ps->getTangent1Z(j)*vz;
			ly = ps->getTangent2X(j)*vx + ps->getTangent2Y(j)*vy + ps->getTangent2Z(j)*vz;
			lz = ps->getNormalX(j)*vx + ps->getNormalY(j)*vy + ps->getNormalZ(j)*vz;
		
			f += (lz - ps->getQuadX(j)*lx*lx - ps->getQuadY(j)*lx*ly - ps->getQuadZ(j)*ly*ly + sol[j+1])*weight(vx*vx + vy*vy + vz*vz);
		}
	}
	return (float)f;
}


void RBF::setSupportConstraint(int minPoints)
{
  minSupport = minPoints;
}

BoundaryTest RBF::testBounds(float x, float y, float z)
{
  if(x < bounds[0] || x > bounds[1] || y < bounds[2] || y > bounds[3] || z < bounds[4] || z > bounds[5])
    return OUTSIDE_BB;
  else
  {
    if(minSupport == 0)
      return INSIDE;
    else
    {
      /*int face1 = (tree->hasNearbyNeighbour(x - boundaryDistance, y, z, boundaryDistance) >= boundaryCount);
      int face2 = (tree->hasNearbyNeighbour(x + boundaryDistance, y, z, boundaryDistance) >= boundaryCount);
      int face3 = (tree->hasNearbyNeighbour(x, y - boundaryDistance, z, boundaryDistance) >= boundaryCount);
      int face4 = (tree->hasNearbyNeighbour(x, y + boundaryDistance, z, boundaryDistance) >= boundaryCount);
      int face5 = (tree->hasNearbyNeighbour(x, y, z - boundaryDistance, boundaryDistance) >= boundaryCount);
      int face6 = (tree->hasNearbyNeighbour(x, y, z + boundaryDistance, boundaryDistance) >= boundaryCount);*/
  
      //if(face1 + face2 + face3 + face4 + face5 + face6 > 2)
      if(numSupportPoints(x, y, z) >= minSupport)
        return INSIDE;
      else
        return OUTSIDE_TIGHT;
    }
  }
}

void RBF::getDimension(float &x, float &y, float &z)
{
  x = bounds[1] - bounds[0];
  y = bounds[3] - bounds[2];
  z = bounds[5] - bounds[4];
}

//Set points to octree (but does not own pointset)
void RBF::setPointSet(PointSet *ps, bool takeOwnership)
{
  if(ownPointSet && ps != NULL)
    delete ps;

	this->ps = ps;
  ownPointSet = takeOwnership;
	tree = new OctTree();
	tree->setPointSet(ps);

	ps->getBound(bounds[0], bounds[1], bounds[2], bounds[3], bounds[4], bounds[5]);

}

//Set sub_rbf and take ownership
void RBF::setSubRBF(RBF *rbf)
{
  if(sub_rbf != NULL)
    delete sub_rbf;

	sub_rbf = rbf;
}

float RBF::getOctreePeak()
{
  return sqrt(tree->getSizeX()*tree->getSizeX() + tree->getSizeY()*tree->getSizeY() + tree->getSizeZ()*tree->getSizeZ());
}


void RBF::splitOctreeChild(int M)
{
  tree->splitChild(M);
}


void RBF::computeOctreeCenter()
{
  tree->computeCenter();
}


inline double RBF::weight(double d2)
{	
	if(T2 < d2)
		return 0;
	
	double r = sqrt(d2/T2);

	return pow(1.0 - r, 4)*(4.0*r + 1.0);
}

void RBF::setSupport(float T)
{
	this->support = T;
	T2 = T*T;
}

//Global interpolation
void RBF::computeSolution(float T)
{
	int point_N = ps->getNumberPoints();
	int total = 0;

	for(int i=0; i<point_N; i++){
		int size = 0;
		tree->getIndexTable(size, ps->getPointX(i), ps->getPointY(i), ps->getPointZ(i), support);
		total += size;
	}

	PBCG solver;
  solver.setSize(total+2);
	double *b = new double[point_N+1];
	if(sol != NULL)
		delete[] sol;
	sol = new double[point_N+1];
  solver.setIja(1, point_N+2);
	int kk=point_N+1;

	for(int i=0; i<point_N; i++){
		solver.setSa(i+1, weight(0) + T);
		double f_sub = 0;
		if(sub_rbf != NULL)
			f_sub = sub_rbf->value(ps->getPointX(i), ps->getPointY(i), ps->getPointZ(i));
		b[i+1] = -f_sub;
		sol[i+1] = -f_sub;

		int size = 0;
		const int* table = tree->getIndexTable(size, ps->getPointX(i), ps->getPointY(i), ps->getPointZ(i), support);
		int* t = new int[size-1];
		int m = 0;
		for(int j=0; j<size; j++){
			if(table[j] == i)
				continue;
			t[m++] = table[j];
		}

		for(int j=0; j<m; j++){
			for(int k=j+1; k<m; k++){
				if(t[j] > t[k]){
					int tmp = t[j];
					t[j] = t[k];
					t[k] = tmp;
				}
			}
		}

		if(!ps->isQuad()){
			for(int j=0; j<m; j++){
				int k = t[j];
				float vx = ps->getPointX(i) - ps->getPointX(k);
				float vy = ps->getPointY(i) - ps->getPointY(k);
				float vz = ps->getPointZ(i) - ps->getPointZ(k);
				double w = weight(vx*vx + vy*vy + vz*vz);
				if(w != 0.0){
					double dot = ps->getNormalX(k)*vx + ps->getNormalY(k)*vy + ps->getNormalZ(k)*vz;
					b[i+1] -= w*dot/sqrt(K2*dot*dot+1.0);
					
					kk++;
          solver.setSa(kk, w);
          solver.setIja(kk, k+1);
				}
			}
		}
		else{
			for(int j=0; j<m; j++){
				int k = t[j];
				float vx = ps->getPointX(i) - ps->getPointX(k);
				float vy = ps->getPointY(i) - ps->getPointY(k);
				float vz = ps->getPointZ(i) - ps->getPointZ(k);
				double w = weight(vx*vx + vy*vy + vz*vz);

				if(w != 0.0){
					double lx = ps->getTangent1X(k)*vx + ps->getTangent1Y(k)*vy + ps->getTangent1Z(k)*vz;
					double ly = ps->getTangent2X(k)*vx + ps->getTangent2Y(k)*vy + ps->getTangent2Z(k)*vz;
					double dot = ps->getNormalX(k)*vx + ps->getNormalY(k)*vy + ps->getNormalZ(k)*vz;
					b[i+1] -= w*(dot - ps->getQuadX(k)*lx*lx - ps->getQuadY(k)*lx*ly - ps->getQuadZ(k)*ly*ly);
					
					kk++;
					solver.setSa(kk, w);
          solver.setIja(kk, k+1);
				}
			}
		}
    solver.setIja(i+2, kk+1);

		if(m != 0)
			delete t;
	}

	int iter;
	double err;
	solver.linbcg(point_N, b, sol, 4, 1e-6, 1000, &iter, &err);

	delete[] b;
}


inline void RBF::weightD(float g[], double d2, float vx, float vy, float vz)
{
	if(T2 < d2 || d2 == 0.0){
		g[0] = g[1] = g[2] = 0;
	}
	double r = sqrt(d2/T2);
	if((float)r == 0){
		g[0] = g[1] = g[2] = 0;
		return;
	}

	double t = -20.0*pow(1.0-r, 3.0)/T2;
	g[0] = (float)(vx * t);
	g[1] = (float)(vy * t);
	g[2] = (float)(vz * t);
}


//call function computeSolution after setting internal data.
void RBF::fit(float support, float ani, float smooth)
{
	this->support = support;
	T2 = support*support;
	K2 = ani*ani;
	this->computeSolution(smooth);
}

//Estimation density of points
float RBF::getOctreeAveragedLeafSize()
{
	return tree->getAverageLeafSize();
}

//Local function computation
void RBF::computeQuad(float support)
{
	this->support = support;
	T2 = support*support;

	int point_N = ps->getNumberPoints();
  ps->setQuad();

	int N = 100;
	float **A = new float*[N+1];
	float **v = new float*[N+1];
	for(int i=1; i<=N; i++){
		A[i] = new float[4];
		v[i] = new float[4];
	}
	float *w = new float[4];
	float *b = new float[N+1];
	float *x = new float[4];

	float e1[3] = {1,0,0};
	float e2[3] = {0,1,0};
  float cp[3];
	for(int i=0; i<point_N; i++){
		const float *n = ps->getNormal(i);

		if(fabs(dotProduct(n, e1)) < fabs(dotProduct(n, e2)))
    {
			crossProduct(cp, n, e1);
      ps->setTangent1X(i, cp[0]);
      ps->setTangent1Y(i, cp[1]);
      ps->setTangent1Z(i, cp[2]);
    }		
    else
    {
			crossProduct(cp, n, e2);
      ps->setTangent1X(i, cp[0]);
      ps->setTangent1Y(i, cp[1]);
      ps->setTangent1Z(i, cp[2]);
    }
		double len = sqrt(ps->getTangent1X(i)*ps->getTangent1X(i) + ps->getTangent1Y(i)*ps->getTangent1Y(i) + ps->getTangent1Z(i)*ps->getTangent1Z(i));
		if((float)len == 0){
      ps->setQuadX(i, 0);
      ps->setQuadY(i, 0);
      ps->setQuadZ(i, 0);
      ps->setTangent2X(i, 0);
      ps->setTangent2Y(i, 0);
      ps->setTangent2Z(i, 0);
			continue;
		}
    ps->setTangent1X(i, ps->getTangent1X(i)/len);
    ps->setTangent1Y(i, ps->getTangent1Y(i)/len);
    ps->setTangent1Z(i, ps->getTangent1Z(i)/len);
		crossProduct(cp, n, ps->getTangent1(i));
    ps->setTangent2X(i, cp[0]);
    ps->setTangent2Y(i, cp[1]);
    ps->setTangent2Z(i, cp[2]);		

		int size;
		const int *index = tree->getIndexTable(size, ps->getPointX(i), ps->getPointY(i), ps->getPointZ(i), support);
		if(size < 4 || size > N){
      ps->setQuadX(i, 0);
      ps->setQuadY(i, 0);
      ps->setQuadZ(i, 0);
			continue;
		}

		int k = 1;
		for(int j=0; j<size; j++){
			if(index[j] == i)
				continue;
			float vx = ps->getPointX(index[j]) - ps->getPointX(i);
			float vy = ps->getPointY(index[j]) - ps->getPointY(i);
			float vz = ps->getPointZ(index[j]) - ps->getPointZ(i);
			float w = (float)weight(vx*vx + vy*vy + vz*vz);

			float x = ps->getTangent1X(i)*vx + ps->getTangent1Y(i)*vy + ps->getTangent1Z(i)*vz;
			float y = ps->getTangent2X(i)*vx + ps->getTangent2X(i)*vy + ps->getTangent2X(i)*vz;
			float z = ps->getNormalX(i)*vx + ps->getNormalY(i)*vy + ps->getNormalZ(i)*vz;
			A[k][1] = w*x*x;
			A[k][2] = w*x*y;
			A[k][3] = w*y*y;
			b[k] = w*z;
			k++;
		}

		SVD::svdcmp(A, size-1, 3, w, v);
		
		float wmax=0.0f;
		for (k=1;k<=3;k++)
			if (fabs(w[k]) > wmax) wmax=(float)fabs(w[k]);
		if(wmax < 0.000001){
      ps->setQuadX(i, 0);
      ps->setQuadY(i, 0);
      ps->setQuadZ(i, 0);
			continue;
		}
		float wmin=wmax*0.01f;
		for (k=1;k<=3;k++){
			if (fabs(w[k]) < wmin) 
				w[k]=0.0;
		}
    
		SVD::svbksb(A, w, v, size-1, 3, b, x);
		
		if(isnan(x[1]) || isnan(x[2]) || isnan(x[3])){
      ps->setQuadX(i, 0);
      ps->setQuadY(i, 0);
      ps->setQuadZ(i, 0);
			continue;
		}
    else
    {
      ps->setQuadX(i, x[1]);
      ps->setQuadY(i, x[2]);
      ps->setQuadZ(i, x[3]);
    }
	}
}

float RBF::valueAndGradient(float g[], float x, float y, float z)
{
	double f = 0;

	if(isGlobal){
		g[0] = 0;
		g[1] = 0;
		g[2] = 0;
		return -1;
	}

	if(sub_rbf != NULL)
		f += sub_rbf->valueAndGradient(g, x, y, z);

	int size, j;
	const int* table = tree->getIndexTable(size, x, y, z, support);
	
	float vx, vy, vz;
	double dot, d2;
	float gw[3];
	double w;
	double g1[3];
	g1[0] = g1[1] = g1[2] = 0;
	double f1, f2;
	if(!ps->isQuad()){
		for(int i=0; i<size; i++){
			j = table[i];
			vx = x - ps->getPointX(j);
			vy = y - ps->getPointY(j);
			vz = z - ps->getPointZ(j);
			dot = ps->getNormalX(j)*vx + ps->getNormalY(j)*vy + ps->getNormalZ(j)*vz;
			d2 = vx*vx + vy*vy + vz*vz;
			weightD(gw, d2, vx, vy, vz);
			w = weight(vx*vx + vy*vy + vz*vz);
		
			f1 = dot/sqrt(K2*dot*dot+1.0) + sol[j+1];
			f2 = (K2*dot*dot-K2*dot+1.0)/pow(K2*dot*dot+1.0, 1.5)*w;

			f += f1*w;
			
			g1[0] += f1*gw[0] + f2*ps->getNormalX(j);
			g1[1] += f1*gw[1] + f2*ps->getNormalY(j);
			g1[2] += f1*gw[2] + f2*ps->getNormalZ(j);
		}
		g[0] += (float)g1[0];
		g[1] += (float)g1[1];
		g[2] += (float)g1[2];		
	}
	else{
		float lx, ly, lz;
		double f1, c1, c2;
		double w, d2;
		for(int i=0; i<size; i++){
			j = table[i];
			vx = x - ps->getPointX(j);
			vy = y - ps->getPointY(j);
			vz = z - ps->getPointZ(j);
			lx = ps->getTangent1X(j)*vx + ps->getTangent1Y(j)*vy + ps->getTangent1Z(j)*vz;
			ly = ps->getTangent2X(j)*vx + ps->getTangent2Y(j)*vy + ps->getTangent2Z(j)*vz;
			lz = ps->getNormalX(j)*vx + ps->getNormalY(j)*vy + ps->getNormalZ(j)*vz;
		
			f1 = lz - ps->getQuadX(j)*lx*lx - ps->getQuadY(j)*lx*ly - ps->getQuadZ(j)*ly*ly + sol[j+1];
			d2 = vx*vx + vy*vy + vz*vz;
			weightD(gw, d2, vx, vy, vz);
			w = weight(vx*vx + vy*vy + vz*vz);

			f += f1*w;
			
			c1 = -2.0f*ps->getQuadX(j)*lx - ps->getQuadY(j)*ly;
			c2 = -2.0f*ps->getQuadZ(j)*ly - ps->getQuadY(j)*lx;

			g1[0] += f1*gw[0] + (ps->getNormalX(j) + c1*ps->getTangent1X(j) + c2*ps->getTangent2X(j))*w;
			g1[1] += f1*gw[1] + (ps->getNormalY(j) + c1*ps->getTangent1Y(j) + c2*ps->getTangent2Y(j))*w;
			g1[2] += f1*gw[2] + (ps->getNormalZ(j) + c1*ps->getTangent1Z(j) + c2*ps->getTangent2Z(j))*w;
		}
		g[0] += (float)g1[0];
		g[1] += (float)g1[1];
		g[2] += (float)g1[2];
	}

	return (float)f;
}

/*bool RBF::getNormal(float x, float y, float z, float delta, float &nx, float &ny, float &nz)
{
  if(!inBounds(x, y, z))
    return false;
  else
  {
	  double f = value(x, y, z);
    nx= value(x+delta, y, z) - f;
    ny = value(x, y+delta, z) - f;
    nz = value(x, y, z+delta) - f;
    f = sqrt(nx*nx + ny*ny + nz*nz);
    
    if (f != 0.0)
    {
      nx /= f; 
      ny /= f; 
      nz /= f;
    }

    return true;
  }
}*/

inline void RBF::crossProduct(float n[3], const float v1[3], const float v2[3])
{
	n[0] = v1[1]*v2[2] - v1[2]*v2[1];
	n[1] = v1[2]*v2[0] - v1[0]*v2[2];
	n[2] = v1[0]*v2[1] - v1[1]*v2[0];
}

inline double RBF::dotProduct(const float v1[3], const float v2[3])
{
	return (double)v1[0]*(double)v2[0] + (double)v1[1]*(double)v2[1] + (double)v1[2]*(double)v2[2];
}




